#include <iostream>
#include <cmath>
#include <GL/glut.h>
#include <set>
#include <cstdlib>
#include <cstdio>
#include "common.h"
#include "meshintersection.h"


int WINDOW_WIDTH = 500, WINDOW_HEIGHT = 500;

// mouse state
int prevX = 0, prevY = 0;
bool leftPressed = false, rightPressed = false, middlePressed = false;

// view state
float rotMat [ 16 ] = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
float focus [ 3 ] = {0,0,-1.5};

const double tiny_value = 1e-10;
const double sq_tiny_value = 1e-10;
const double large_value = 1e15; 

//const double sr_tiny_value = 1e-5; 
//const double tiny_value = 1e-5 ;

extern MeshType mesh1 ; 
extern MeshType mesh2 ;
extern MeshType mesh3 ;

//extern std::vector<std::deque<Vector <double> > > v_curve; 
//extern std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;

Box <Value> box; 
bool frame = false; 
bool gb_number = false; 
bool gb_vertex = false; 
bool gb_light  = false; 


void draw_text(GLfloat x, GLfloat y, GLfloat z,  char *text)
{
    char *p = text; 
    glPushMatrix();
    glTranslatef(x, y, z);
	double scale = 0.0005f;
	glScalef(scale, scale, scale);
	//glScalef(0.0001f, 0.0001f, 0.0001f);
    for (p = text; *p; p++)
        glutStrokeCharacter(GLUT_STROKE_ROMAN, *p);
    glPopMatrix();
}

void draw_number (GLfloat x, GLfloat y, GLfloat z,  int num)
{
	char array[255] ;
	sprintf (array, "%d", num);
	draw_text (x, y, z, array); 
}

void draw_triangles_facet (const std::vector<PointType *> & point, const VectorType & normal, int faceid	,	std::vector<int> pv)
{
	PointType p (0.0f, 0.0f, 0.0f);

	glBegin(GL_POLYGON);
	glNormal3f(GLFLOAT(normal[0]), GLFLOAT(normal[1]), GLFLOAT(normal[2])); 
	for (unsigned i = 0; i < point.size(); ++i)
	{
		p += (*point[i]) ;
		glVertex3f( GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]));

		
	}

	p /= Value(point.size());
	glEnd();
	if (gb_number)
	{
		draw_number (p[0], p[1], p[2], faceid);
	}

	for (unsigned i = 0; i < point.size(); ++i)
	{
		if (gb_vertex)
		{
			draw_number (GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]), pv[i]);
		}
	}
}

void draw_AABBTree_polygon (const AABBTreePolygonType & polygon,  const std::vector<PointType > & point)
{

	glBegin(GL_POLYGON);
//	glNormal3f(normal[0], normal[1], normal[2]); 
	for (unsigned i = 0; i < polygon.get_nb_vertices(); ++i)
	{
		PointType p = point[polygon.getIndex (i)];
		glVertex3f (GLFLOAT(p[0]), GLFLOAT(p[1]), GLFLOAT(p[2]));
	}
	glEnd();

}

void draw_polygon (const PolygonType & polygon)
{

	glBegin(GL_POLYGON);
//	glColor3f (0, 1.0, 1.0);
//	glNormal3f(normal[0], normal[1], normal[2]); 
	for (unsigned i = 0; i < polygon.get_nb_vertices(); ++i)
	{
		PointType p = polygon[i];
		glVertex3f (GLFLOAT(p[0]), GLFLOAT(p[1]), GLFLOAT(p[2]));
	}
	glEnd();

}

void draw_box_intersection_result (const AABBTreePolyType::BoxColResult & res, const std::vector<PointType > & point)
{
	for (unsigned i = 0; i < res.polys_.size(); ++i)
	{
		draw_AABBTree_polygon(*res.polys_[i], point);
	}
}
void draw_segment_intersection_result (const AABBTreePolyType::SegmentColResult & res, const std::vector<PointType > & point)
{
	for (unsigned i = 0; i < res.polys_.size(); ++i)
	{
		//draw_AABBTree_polygon(*res.polys_[i], point);
		std::vector<PointType *> p;
		int face_id = (res.polys_[i])->get_face_idx(); 
		mesh1.faces[face_id]->Face2VertexCoord(p) ;
		std::vector<int> pv;
		mesh1.faces[face_id]->Face2VertexIdx(pv);
		draw_triangles_facet (p, mesh1.faces[face_id]->normal(), face_id, pv);
	}
}


void draw_curve (MeshType & mesh)
{
	typedef std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator map_iter;
	glColor3f(0.0, 1.0, 1.0);
	for (map_iter iter = mesh.map_mergeInter.begin(); iter!= mesh.map_mergeInter.end(); ++iter)
	{
		switch (iter->second.type)
		{
		case SEGMENT :
			glBegin(GL_LINE_STRIP); 
			glVertex3f(GLFLOAT(iter->second.segment.p0[0]), GLFLOAT(iter->second.segment.p0[1]), GLFLOAT(iter->second.segment.p0[2]));
			glVertex3f(GLFLOAT(iter->second.segment.p1[0]), GLFLOAT(iter->second.segment.p1[1]), GLFLOAT(iter->second.segment.p1[2]));
			glEnd();
			break;
			;
		case POLYGON:
			break;
			throw ("Display Polygon Error");

		default:
			throw ("Unknow interseciton type");
		}
	}
	glColor3f(1.0, 1.0, 1.0);
}
/*
void draw_curve ()
{
	for (unsigned i = 0; i < v_curve.size(); ++i)
	{
			glBegin(GL_LINE_STRIP); 
			glColor3f(0, 1.0, 0);
			for (unsigned j = 0; j < v_curve[i].size(); ++j)
			{
				glVertex3f(GLFLOAT(v_curve[i][j][0]), GLFLOAT(v_curve[i][j][1]), GLFLOAT(v_curve[i][j][2]));
			}
			glEnd();
	}

		
}
*/

void draw_segment (const PointType & p1, const PointType & p2)
{
	glBegin(GL_LINES);
	glVertex3f (GLFLOAT(p1[0]), GLFLOAT(p1[1]), GLFLOAT(p1[2]));
	glVertex3f (GLFLOAT(p2[0]), GLFLOAT(p2[1]), GLFLOAT(p2[2]));
	glEnd();
}
void draw_mesh_intersection_result (MeshType * mesh)
{

	glBegin(GL_LINES);
	glColor3f (0, 1.0, 0);
	for (unsigned i = 0; i < mesh->v_segment.size(); ++i)
	{
		glVertex3f (GLFLOAT(mesh->v_segment[i].p0[0]), GLFLOAT(mesh->v_segment[i].p0[1]), GLFLOAT(mesh->v_segment[i].p0[2]));
		glVertex3f (GLFLOAT(mesh->v_segment[i].p1[0]), GLFLOAT(mesh->v_segment[i].p1[1]), GLFLOAT(mesh->v_segment[i].p1[2]));
	}
	glEnd();
	
	for (unsigned i = 0; i < mesh->v_polygon.size(); ++i)
	{
		draw_polygon(mesh->v_polygon[i]);
	}
	
}
/*
  void draw_triangles_smooth (Point point[3], Vector3 normal[3])
  {
  glNormal3f(normal[0].x, normal[0].y, normal[0].z); 
  glVertex3f(point[0].x, point[0].y, point[0].z);
  glNormal3f(normal[1].x, normal[1].y, normal[1].z); 
  glVertex3f(point[1].x, point[1].y, point[1].z);
  glNormal3f(normal[2].x, normal[2].y, normal[2].z); 
  glVertex3f(point[2].x, point[2].y, point[2].z);

  }
*/



void draw_one_facet (MeshType * mesh, int idx)
{
	glColor3f (1.0, 0.0, 0.0); 
	glLineWidth(4.0);
	std::vector<PointType *> p;
	mesh->faces[idx]->Face2VertexCoord(p); 
	std::vector<int> pv;
	mesh->faces[idx]->Face2VertexIdx(pv);
	draw_triangles_facet ( p, mesh->faces[idx]->normal(), idx, pv); 
	glLineWidth(1.0);
}

void draw_mesh_facet (MeshType * mesh, int idx)
{
	for (unsigned i = 0; i < mesh->faces.size(); ++i)
	{
		//if (mesh->faces[i]->m_idx != idx) continue; 
		std::vector<PointType *> p;
		mesh->faces[i]->Face2VertexCoord(p); 
		std::vector<int> pv;
		mesh->faces[i]->Face2VertexIdx(pv);
		//glColor3f(1.0,1.0,1.0f);
		draw_triangles_facet ( p, mesh->faces[i]->normal(), i, pv); 
	}	
}

void draw_new_face (MeshType * mesh, int idx)
{

//	std::cout<<"New Face Size"<<v_new_face_id.size() <<std::endl;
	glLineWidth(2.0);
	for (unsigned i = 0; i < mesh->v_new_face_id.size(); ++i)
	{
//		if (mesh->faces[i]->m_idx != idx) continue; 
		std::vector<PointType *> p;
		mesh->faces[mesh->v_new_face_id[i]]->Face2VertexCoord(p); 
		//glColor3f(1.0,1.0,1.0f);
		std::vector<int> pv;
		mesh->faces[mesh->v_new_face_id[i]]->Face2VertexIdx(pv);
		draw_triangles_facet ( p, mesh->faces[mesh->v_new_face_id[i]]->normalVector, mesh->v_new_face_id[i], pv); 
	}
	glLineWidth(1.0);	
}

void draw_mesh_smooth (MeshType * mesh)
{
	/*
	  glBegin(GL_TRIANGLES);
	  for (unsigned i = 0; i < mesh->v_faces.size(); ++i)
	  {
	  Face * face = mesh->v_faces[i] ;
	  Point p [3]; 
	  Point n [3];
	  face->Face2VertexNormal(n);
	  face->Face2VertexCoord(p); 
	  //n = face->normal();
	  draw_triangles_smooth ( p, n); 
	  }
	  glEnd();
	*/
}  

AABBTreePolyType::BoxColResult boxColResult;
AABBTreePolyType::SegmentColResult segmentColResult;
std::vector <PointType> vertices ; 


void draw_box (const BoxType & box)
{
	glBegin(GL_LINES);
	glColor3f (0, 1.0, 0);
	SegmentType segment ;
	for (int i = 0; i < 12; ++i)
	{
		segment = box.getEdge(i);
		glVertex3f (GLFLOAT(segment.p0[0]), GLFLOAT(segment.p0[1]), GLFLOAT(segment.p0[2]));
		glVertex3f (GLFLOAT(segment.p1[0]), GLFLOAT(segment.p1[1]), GLFLOAT(segment.p1[2]));
	}
	glEnd();
}
void draw_aabbtree (const AABBTreeNodeType * node)
{
	if (node == NULL) return ;
	draw_box (node->aabb);
	draw_aabbtree(node->left);
	draw_aabbtree(node->right);
} 

void display() 
{
	if (gb_light)
	{
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHTING);
		glEnable(GL_DEPTH_TEST);	
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}
	else
	{
		glDisable(GL_LIGHT0);
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	}

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

	// Set up viewing matrices
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60, float(WINDOW_WIDTH)/WINDOW_HEIGHT, .0001, 100);
	glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
	glPushMatrix();



	//Camera
	glTranslatef(focus[0], focus[1], focus[2]);
	glMultMatrixf(rotMat);


	// drawing code goes here
	//glutSolidCube ( 1 );
	//draw_mesh_smooth (mesh);
	glColor3f(1.0f, 1.0f, 1.0f);
	
	// Comment from here to see
	/*
	PointType p = VectorType (0.962775,-0.135328,-0.345057);
	PointType d = VectorType (-0.108214,-0.770402,-0.628308);
	draw_segment (p , p + d );
	*/
	//draw_aabbtree(mesh1.get_aabbtree()->root);
//	draw_new_face ( &mesh1, 26);
//	draw_new_face ( &mesh2, 26);
//	draw_curve (mesh1);
//	draw_curve (mesh2);
	if (frame )
	{
		//draw_new_face ( &mesh1, 26);
		//draw_new_face ( &mesh2, 26);

		glLineWidth(2.0);
		glColor3f(1.0f, 0.0f, 0.0f);
		draw_mesh_facet (&mesh1, 264);
		glLineWidth(2.0);
		glColor3f(0.0f, 1.0f, 0.0f);
		draw_mesh_facet (&mesh2, 226);

//		draw_mesh_facet (&mesh3, 26);

//		draw_one_facet (&mesh1, 264);
//		draw_one_facet (&mesh2, 234);
	}
	else
	{
		glColor3f(1.0f, 0.0f, 0.0f);
		draw_new_face ( &mesh1, 26);
		glColor3f(0.0f, 1.0f, 0.0f);
		draw_new_face ( &mesh2, 26);
		glColor3f(1.0f, 1.0f, 1.0f);
	}

	//glColor3f(1.0f, 0.0f, 0.0f);
	
	//draw_mesh_intersection_result(&mesh2);

	// end drawing code


	//draw_aabbtree (tree->root);
	//draw_aabbtree(mesh1.get_aabbtree()->root);

	glPopMatrix();
	glFlush ();
	glutSwapBuffers();

}

void reshape(int width, int height)
{
	glViewport(0, 0, width, height);
	WINDOW_WIDTH = width;
	WINDOW_HEIGHT = height;
}

void mouse(int button, int state, int x, int y)
{
	y = WINDOW_HEIGHT - y;

	// Mouse state that should always be stored on pressing
	if (state == GLUT_DOWN)
	{
		prevX = x;
		prevY = y;
	}

	if (button == GLUT_LEFT_BUTTON)
	{
		leftPressed = state == GLUT_DOWN;
	}

	if (button == GLUT_RIGHT_BUTTON)
	{
		rightPressed = state == GLUT_DOWN;
	}
	
	if (button == GLUT_MIDDLE_BUTTON)
	{
		middlePressed = state == GLUT_DOWN;
	}
}

void motion(int x, int y)
{
	y = WINDOW_HEIGHT - y;

	float dx = (x - prevX);
	float dy = (y - prevY);

	// rotate the scene
	if (leftPressed)
	{
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glRotatef(dx, 0, 1, 0);
		glRotatef(dy, -1, 0, 0);
		glMultMatrixf(rotMat);
		glGetFloatv(GL_MODELVIEW_MATRIX, rotMat);
	}
	else if (middlePressed)
	{
		focus [ 0 ] += 0.005 * dx;
		focus [ 1 ] += 0.005 * dy;
	}
	else if (rightPressed)
	{
		focus [ 2 ] += 0.01 * dy;
	}
	
	// Store previous mouse positions
	prevX = x;
	prevY = y;

	glutPostRedisplay();
}


void keyboard(unsigned char key, int x, int y)
{
	if (key == 'f' || key == 'F') frame = !frame ; 
	if (key == 'b' || key == 'B') gb_number = !gb_number ;
	if (key == 'v' || key == 'V') gb_vertex = !gb_vertex; 
	if (key == 'l' || key == 'L') gb_light = !gb_light; 
	glutPostRedisplay();
}

void output_face (FaceType * face)
{
	FaceType::FVIter iter (face); 
	for (iter = face->fv_begin(); iter != face->fv_end(); ++iter)
	{
		std::cout<<(*iter)->m_coord<<' ';
	}
}

void init(void)
{
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glViewport(0, 0, WINDOW_WIDTH, WINDOW_HEIGHT);

	glClearColor(0, 0, 0, 1);   

	GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
	GLfloat mat_shininess[] = { 50.0 };
	//GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
	glClearColor (0.0, 0.0, 0.0, 0.0);
	glShadeModel (GL_SMOOTH);

	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
	//glLightfv(GL_LIGHT0, GL_POSITION, light_position);





	
	//glEnable(GL_CULL_FACE);
	/*


	*/
	//glPolygonMode(GL_FRONT, GL_LINE);

}


int main(int argc, char **argv)
{

	try 
	{
		
//		std::ifstream ifs1 (argv[1]);
//		std::ifstream ifs2 (argv[1]);
		//std::ifstream ifs1 ("../data/unitplane-x-y.obj");
		//std::ifstream ifs2 ("../data/generalplane.obj");

		//ifs1: cube3, ifs2: cube1. Wrong sort of points around vertex. 
		std::ifstream ifs1 ("../data/cube3.obj");
		//std::ifstream ifs2 ("../data/bigArmadillo_20qem.obj");
	    //std::ifstream ifs1 ("../data/sphere.obj");
		//std::ifstream ifs2 ("../data/cossphere.obj");
		std::ifstream ifs2 ("../data/cube1.obj.2");
		//std::ifstream ifs2 ("../data/kitten_manifold_4k.obj");
		//std::ifstream ifs2 ("../data/torus.obj");

		//std::ifstream ifs1 ("../data/cube3.obj");
		//std::ifstream ifs2 ("../data/cube3.obj.2");


		//std::ifstream ifs1 ("../data/unitplane.obj");
		//std::ifstream ifs2 ("../data/unitplane2.obj");

		//std::ifstream ifs1 ("../data/unitplane-x-z.obj");
		//std::ifstream ifs2 ("../data/unitplane-x-z.obj2");

		//std::ifstream ifs1 ("../data/unitcube.obj");
		//std::ifstream ifs2 ("../data/unitcube.obj2");

		//std::ifstream ifs1 ("../data/single.obj");
		//std::ifstream ifs2 ("../data/single.obj2");

		//std::ifstream ifs1 ("../data/single.obj");
		//std::ifstream ifs2 ("../data/single.obj2");

		//std::ifstream ifs1 ("../data/quadcube.obj");
		//std::ifstream ifs2 ("../data/quadcube.obj2");


		ifs1 >> (mesh1) ;
		std::cout<<mesh1.faces.size()<<std::endl;

	
		ifs2 >> (mesh2) ;
		std::cout<<mesh2.faces.size()<<std::endl;		

		
		//tree = buildAABBTree (mesh2);

		
		//VectorType v(Value(0.40), Value(0.47), Value(0.45));
		//VectorType v(Value(-0.40), Value(0.0), Value(0.45));
		//VectorType v(Value(0.0), Value(0.0), Value(0.45));
		//VectorType v(Value(0.43), Value(0.43), Value(0.45));
		//VectorType v(Value(0.4), Value(0.4), Value(0.4));
		//VectorType v(Value(0.21), Value(0.2), Value(0.2)); //Kitten
		//VectorType v(Value(0.33), Value(0.0), Value(0.33)); //Kitten
		VectorType v(Value(-0.2), Value(0.4), Value(0.0)); //Kitten
		//VectorType v(Value(-0.2), Value(0.4), Value(0.0)); 
		mesh2.translate (v);

		mesh1.build_aabbtree();
		mesh2.build_aabbtree();

		//output_face(mesh1.faces[5]);
		//output_face(mesh2.faces[5]);
		
		//mesh1.rotateX(Value(0.1));
		//mesh2.rotateX(Value(0.1));

		// /When swap this mesh order, the displayed mesh should be changed. 

		LATENCY (mesh_intersection(mesh1, mesh2), "Mesh Intersection");

		ofstream ofs("mesh3.obj");
		mesh3.OBJ_writer (ofs);

		//mesh1.copy_mesh(mesh3) ;

		std::cout<<"FINISH INTERSECTION "<<std::endl;
		std::cout<<"MESH1 FACE NUM "<<mesh1.v_face_id.size()<<std::endl;
		std::cout<<"MESH1 SEGMENT N "<<mesh1.v_segment.size()<<std::endl;
		std::cout<<"MESH1 POLYGON N "<<mesh1.v_polygon.size()<<std::endl;
		std::cout<<"MESH1 EDGES   N "<<mesh1.v_edges.size()<<std::endl;


		std::cout<<"MESH3 FACE NUM "<<mesh3.faces.size()<<std::endl;
		std::cout<<"MESH3 VERTEX N "<<mesh3.vertices.size()<<std::endl;
		std::cout<<"MESH3 EDGES  N "<<mesh3.edges.size()<<std::endl;
		/*
		for (std::set<int>::iterator iter = v_face_id.begin(); iter != v_face_id.end(); ++iter)
		{
			std::cout<<(*iter)<<' ';
		}
		*/
		std::cout<<std::endl;

	}
	catch (const char * errmsg)
	{
		std::cerr <<errmsg<<std::endl;
	}

	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInitWindowPosition(0, 0);
	glutInitWindowSize(WINDOW_WIDTH, WINDOW_HEIGHT);
	glutCreateWindow("Field detection.");

	init();

	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);

	// main loop
	glutMainLoop();

	return 0;
}
